% Copyright (c) 2010, John Gebbie. All rights reserved.
%
% This library is free software; you can redistribute it and/or modify it
% under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation; either version 2.1 of the License, or (at
% your option) any later version.
%
% This library is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
% General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with this library; if not, write to the Free Software Foundation,
% Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
%
%
% bf3d.m - produce 3-D beam patterns for simple delay-and-sum beamformer

%% init
clear;
clc;

%% input parameters
make_avi=false;
N_gridpoints=200;

if make_avi
    close all;
end

hFig=figure(1);
clf(hFig);
window_pos=[1 3];
window_dim=[5 5];
set(hFig,'Units','inches');
set(hFig,'Position',[window_pos window_dim]);

N=23;                               % number of receivers per array
d_quart=.25;                        % vertical element separation in meters
d_half=.5;                          % horizontal element separation in meters
c=1500;                             % sound speed in water
fr_design=c/(2*d_half);             % design frequency of longest array
L_quart=(N-1)*d_quart;
L_half=(N-1)*d_half;
c_quart=(0:N-1).'.*d_quart;
c_half=(0:N-1).'.*d_half;


% %%%% no looping
% a_phi=0;
% a_theta=pi/3;
% fr=fr_design/2;
% for ix=1:1

% %%%% loop over frequencies
% a_phi=0;
% a_theta=pi;
% freqs=linspace(50,fr_design*4,100);
% for ix=1:length(freqs), fr=freqs(ix);

%%%% loop over theta (zenith)
fr=fr_design/2;
a_phi=0;
a_thetas=linspace(0,2*pi,300);
for ix=1:length(a_thetas), a_theta=a_thetas(ix);
    
    % %%%% loop over phi (azimuth)
    % fr=fr_design/2;
    % a_theta=pi/4;
    % a_phis=linspace(0,2*pi,300);
    % for ix=1:length(a_phis), a_phi=a_phis(ix);
    
    %% array geometry
    if ix==1
        
        % %%%% One line array geometry
        % N=N+1; % have one extra element if using one array
        % p_vert=[zeros(N,1) zeros(N,1) ((0:N-1)'*d_quart)];
        % p=p_vert;
        % p_origin=[0 0 0];
        % movie_filename='oneline';
        
        % %%%% T-shaped geometry
        % p_vert=[zeros(N,1) zeros(N,1) (c_quart+d_quart)];
        % p_horiz=[(c_half-L_half/2) zeros(N,1) zeros(N,1)];
        % p=[p_vert;p_horiz];
        % p_origin=[0 0 0];
        % movie_filename='tshape';
        
        % %%%% Angle on bottom
        % p_ang=pi/2;
        % p_xy_vert=[0 0]; %p_xy_vert=[L_half/4 0];
        % p_vert=[repmat(p_xy_vert(1),N,1) repmat(p_xy_vert(2),N,1) c_quart+d_quart];
        % p_leg1=c_half(1:floor(N/2))+d_half;
        % p_leg2=c_half(1:ceil(N/2));
        % p_leg1=[p_leg1*cos( p_ang/2) p_leg1*sin( p_ang/2) zeros(size(p_leg1))];
        % p_leg2=[p_leg2*cos(-p_ang/2) p_leg2*sin(-p_ang/2) zeros(size(p_leg2))];
        % p=[p_vert;p_leg1;p_leg2];
        % p_origin=[0 0 0];
        % movie_filename='angle';
        
        % %%%% L-shaped geometry
        % p_vert=[zeros(N,1) zeros(N,1) (c_quart+d_quart)];
        % p_horiz=[(-c_half) zeros(N,1) zeros(N,1)];
        % p=[p_vert;p_horiz];
        % p_origin=[0 0 0];
        % movie_filename='lshape';
        
        % %%%% cross-shaped geometry
        % p_vert=[zeros(N,1) zeros(N,1) (c_quart+d_quart)];
        % p_horiz=[(c_half-L_half/2) zeros(N,1) (zeros(N,1)+L_quart/2)];
        % p=[p_vert;p_horiz];
        % p_origin=[0 0 N/2*d_quart];
        % movie_filename='cross';
        
        % %%%% Parallel vertical
        % p_spacing=1;
        % p_left=[repmat(-p_spacing/2,N,1) zeros(N,1) c_quart];
        % p_right=[repmat(p_spacing/2,N,1) zeros(N,1) c_half];
        % p=[p_left;p_right];
        % p_origin=[0 0 0];
        % movie_filename='parallel2m';
        
        % %%%% Circle on bottom
        % p_vert=[zeros(N,1) zeros(N,1) c_quart];
        % p_angs=(0:N-1)'./N.*2*pi;
        % p_rad=L_half./(2.*pi);
        % p_elev=0; %p_elev=N/2.*d_quart;
        % p_circ=[p_rad.*cos(p_angs) p_rad.*sin(p_angs) (zeros(N,1)+p_elev)];
        % p=[p_vert;p_circ];
        % p_origin=[0 0 p_elev];
        % movie_filename='circ';
        
        % %%%% Spiral on bottom
        % p_rotations=1;
        % p_vert=[zeros(N,1) zeros(N,1) c_quart];
        % p_dphi=1/N*2*pi*p_rotations;
        % p_rho=sqrt((d_half^2-d_quart^2)./(2*(1-cos(p_dphi)))); % ensure spacing correct
        % p_angs=(0:N-1)'.*p_dphi;
        % p_circ=[p_rho.*cos(p_angs) p_rho.*sin(p_angs) c_quart];
        % p=[p_vert;p_circ];
        % p_origin=[0 0 0];
        % movie_filename='spiral';
        
        % %%%% Grid (not realizable with our hardware)
        % p_nelt=10;
        % [p_x,p_y]=ndgrid(d_half*(-p_nelt/2:p_nelt/2-1),d_half*(-p_nelt/2:p_nelt/2-1));
        % p=[ reshape(p_x,[],1), reshape(p_y,[],1) zeros(length(p_x(:)),1)];
        % p_origin=[0 0 0];
        % movie_filename='grid';
        
        % %%%% Cylinder (not realizable with our hardware)
        % p_nelt=10;
        % p_rad=(p_nelt-1).*d_half./(2.*pi);
        % [p_angs,p_zs]=ndgrid((0:p_nelt-1)'./p_nelt.*2*pi,(0:p_nelt-1).*d_half);
        % p=[ reshape(p_rad.*cos(p_angs),[],1), reshape(p_rad.*sin(p_angs),[],1), reshape(p_zs,[],1)];
        % p_origin=[0 0 0];
        % movie_filename='cylinder';
        
        % %%%% Sphere (not realizable with our hardware)
        % p_radius=3*d_half;
        % p_thetas=linspace(0,pi,ceil(2*pi*p_radius./d_half));
        % p=[];
        % for p_ix_thetas=1:length(p_thetas)
        %     p_rho=p_radius.*sin(p_thetas(p_ix_thetas));
        %     p_phis=linspace(0,2*pi,ceil(2*pi*p_rho./d_half));
        %     for ix_phis=1:length(p_phis)
        %         p(end+1,:)=[p_rho.*cos(p_phis(ix_phis)) p_rho.*sin(p_phis(ix_phis)) p_radius.*cos(p_thetas(p_ix_thetas))];
        %     end
        % end
        % p_origin=[0 0 0];
        % movie_filename='sphere';
        
        %%%% Tetrahedron (not realizable with our hardware)
        p_nelt=5;
        p_tet_angle = acos(-1/3);
        p=[ 0, 0, 0 ;...
            0, 0, 1 ;...
            cos(0*p_tet_angle)*sin(p_tet_angle), sin(0*p_tet_angle)*sin(p_tet_angle), cos(p_tet_angle) ;...
            cos(1*p_tet_angle)*sin(p_tet_angle), sin(1*p_tet_angle)*sin(p_tet_angle), cos(p_tet_angle) ;...
            cos(2*p_tet_angle)*sin(p_tet_angle), sin(2*p_tet_angle)*sin(p_tet_angle), cos(p_tet_angle) ];
        p_origin=[0 0 0];
        movie_filename='tetrahedron';
        
    end
    
    %% center the array at the specified origin
    p=p-repmat(p_origin,size(p,1),1);
    p_scale=max(sqrt(sum(p.^2,2))); %scaling factor (for plotting)
    
    %% incident planewave as unit vector
    a=[sin(a_theta).*cos(a_phi) sin(a_theta).*sin(a_phi) cos(a_theta)]';
    
    %% compute look angles over grid of points
    if ix==1
        lk_phi=linspace(0,2*pi,N_gridpoints);
        lk_theta=linspace(0,pi,N_gridpoints);
        [LK_PHI,LK_THETA]=ndgrid(lk_phi,lk_theta);
        LK_X=sin(LK_THETA).*cos(LK_PHI);
        LK_Y=sin(LK_THETA).*sin(LK_PHI);
        LK_Z=cos(LK_THETA);
        lk=[reshape(LK_X,1,[]);reshape(LK_Y,1,[]);reshape(LK_Z,1,[])];
        
        wd=p*lk;                % dot product of element position and look direction
        wm=ones(size(wd));      % steering vector element weight
        
        % % weight elements according to distribution along line parallel to
        % % direction of look angle
        % wm=wd;
        % wmsort=nan(size(wm));
        % wmsortix=nan(size(wm));
        % for jx=1:size(wmsort,2)
        %     [wmsort(:,jx) wmsortix(:,jx)]=sort(wm(:,jx));
        % end
        % wm2=wmsort(1:end-1,:)+diff(wmsort,1,1)./2;
        % wm3=[2*(wm2(1,:)-wmsort(1,:)) ; diff(wm2) ; 2*(wmsort(end,:)-wm2(end,:))];
        % for jx=1:size(wmsort,2)
        %     wm(wmsortix(:,jx),jx)=wm3(:,jx);
        % end
    end
    
    %% do beam-forming
    w=2.*pi.*fr;
    k=w./c;
    w=wm.*exp(-1i.*k.*wd);              % steering vector
    f=exp(-1i.*k.*(p*a));               % measured response of incident planewave
    bf=w'*f;                            % conventional beamforming
    
    bf=reshape(bf,size(LK_X));          % normalize
    bfn=abs(bf)./max(abs(bf(:)));
    
    bfn=max(20+20*log10(bfn),0);        % put in db scale, crop at 20dB
    bfn=bfn./max(bfn(:));
    
    bfn=bfn.*p_scale./2;                % scale relative to size of array (for plotting)
    
    %% plot
    mesh(bfn.*LK_X,bfn.*LK_Y,bfn.*LK_Z,bfn,...
        'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
    caxis([0 max(bfn(:))]);
    hold on;
    scatter3(p(:,1),p(:,2),p(:,3));
    aline=[0 0 0;a.'./sqrt(sum(a.^2)).*p_scale];
    plot3(aline(:,1),aline(:,2),aline(:,3),'r','LineWidth',2);
    hold off;
    axis equal;
    xlabel('x');
    ylabel('y');
    zlabel('z');
    xlim([-max(abs(p(:))) max(abs(p(:)))]);
    ylim([-max(abs(p(:))) max(abs(p(:)))]);
    zlim([-max(abs(p(:))) max(abs(p(:)))]);
    camva(3); % zoom in a bit
    
    if make_avi
        frames(ix)=getframe(hFig);
    else
        drawnow();
        pause(.01);
    end
    
end

% and... action!
if make_avi
    movie2avi(frames,[movie_filename '.avi'],'compression','None');
end
